from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
import rclpy
import math

def main():
    rclpy.init()
    nav = BasicNavigator() #节点
    nav.waitUntilNav2Active() #等待导航可用

    goal_poses=[]
    # goal_pose = PoseStamped()
    # goal_pose.header.frame_id="map"
    # goal_pose.header.stamp=nav.get_clock().now().to_msg()
    # goal_pose.pose.position.x=1.0
    # goal_pose.pose.position.y=2.0
    # goal_pose.pose.orientation.w = 1.0
    # goal_poses.append(goal_pose)

    goal_pose = PoseStamped()
    goal_pose.header.frame_id="map"
    goal_pose.header.stamp=nav.get_clock().now().to_msg()
    goal_pose.pose.position.x=3.0
    goal_pose.pose.position.y=2.0
    goal_pose.pose.orientation.x = 0.0
    goal_pose.pose.orientation.y = 0.0
    goal_pose.pose.orientation.z = -math.sin(math.pi/4)  # ≈-0.7071
    goal_pose.pose.orientation.w = math.cos(math.pi/4)   # ≈0.7071
    goal_poses.append(goal_pose)

    goal_pose2 = PoseStamped()
    goal_pose2.header.frame_id="map"
    goal_pose2.header.stamp=nav.get_clock().now().to_msg()
    goal_pose2.pose.position.x=1.5
    goal_pose2.pose.position.y=-1.5
    goal_pose2.pose.orientation.w=1.0
    goal_poses.append(goal_pose2)

    goal_pose3 = PoseStamped()
    goal_pose3.header.frame_id="map"
    goal_pose3.header.stamp=nav.get_clock().now().to_msg()
    goal_pose3.pose.position.x=3.0
    goal_pose3.pose.position.y=-1.0
    goal_pose3.pose.orientation.x = 0.0
    goal_pose3.pose.orientation.y = 0.0
    goal_pose3.pose.orientation.z = math.sin(math.pi/4)  # ≈0.7071
    goal_pose3.pose.orientation.w = math.cos(math.pi/4)  # ≈0.7071
    goal_poses.append(goal_pose3)


    goal_pose4 = PoseStamped()
    goal_pose4.header.frame_id="map"
    goal_pose4.header.stamp=nav.get_clock().now().to_msg()
    goal_pose4.pose.position.x=-3.0
    goal_pose4.pose.position.y=0.0
    goal_pose4.pose.orientation.z = 1.0 
    goal_poses.append(goal_pose4)

    goal_pose5 = PoseStamped()
    goal_pose5.header.frame_id="map"
    goal_pose5.header.stamp=nav.get_clock().now().to_msg()
    goal_pose5.pose.position.x=-1.5
    goal_pose5.pose.position.y=1.75
    goal_pose5.pose.orientation.w = 1.0 
    goal_poses.append(goal_pose5)

    goal_pose6 = PoseStamped()
    goal_pose6.header.frame_id="map"
    goal_pose6.header.stamp=nav.get_clock().now().to_msg()
    goal_pose6.pose.position.x=-3.0
    goal_pose6.pose.position.y=0.0
    goal_pose6.pose.orientation.w = 1.0 
    goal_poses.append(goal_pose6)

    goal_pose7 = PoseStamped()
    goal_pose7.header.frame_id="map"
    goal_pose7.header.stamp=nav.get_clock().now().to_msg()
    goal_pose7.pose.position.x=-3.0
    goal_pose7.pose.position.y=-1.5
    goal_pose7.pose.orientation.z = 1.0 
    goal_poses.append(goal_pose7)

    nav.followWaypoints(goal_poses)
    while not nav.isTaskComplete():
        feedback=nav.getFeedback()
        nav.get_logger().info(f'路点编号{feedback.current_waypoint}')
        #nav.cancelTask()
    result=nav.getResult()
    nav.get_logger().info(f'导航结果:{result}')

    # rclpy.spin(nav)
    # rclpy.shutdown()
